
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ModeFilterFloat5.c
  * @author     baiyang
  * @date       2023-7-27
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ModeFilter.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void mff5_isort(mmf5_t filter, float new_sample, bool drop_high);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mff5_ctor(mmf5_t filter, uint8_t return_element)
{
    filter->_return_element  = return_element;
    filter->drop_high_sample = true;

    // ensure we have a valid return_nth_element value.  if not, revert to median
    if (filter->_return_element >= MODE_FILTER_SIZE_5 ) {
        filter->_return_element = MODE_FILTER_SIZE_5 / 2;
    }
}

// apply - Add a new raw value to the filter, retrieve the filtered result
float mff5_apply(mmf5_t filter, float sample)
{
    // insert the new items into the samples buffer
    mff5_isort(filter, sample, filter->drop_high_sample);

    // next time drop from the other end of the sample buffer
    filter->drop_high_sample = !filter->drop_high_sample;

    // return results
    if (filter->sample_index < MODE_FILTER_SIZE_5 ) {
        // middle sample if buffer is not yet full
        filter->_output = filter->samples[(filter->sample_index / 2)];
    } else {
        // return element specified by user in constructor
        filter->_output = filter->samples[filter->_return_element];
    }

    return filter->_output;
}

// reset - clear the filter
void mff5_reset(mmf5_t filter)
{
    // clear samples buffer
    for(int8_t i=0; i<MODE_FILTER_SIZE_5; i++ ) {
        filter->samples[i] = 0;
    }

    // reset index back to beginning of the array
    filter->sample_index = 0;
}

static void mff5_isort(mmf5_t filter, float new_sample, bool drop_high)
{
    int8_t        i;

    // if the buffer isn't full simply increase the #items in the buffer (i.e. sample_index)
    // the rest is the same as dropping the high sample
    if ( filter->sample_index < MODE_FILTER_SIZE_5 ) {
        filter->sample_index++;
        drop_high = true;
    }

    if ( drop_high ) {     // drop highest sample from the buffer to make room for our new sample

        // start from top. Note: sample_index always points to the next open space so we start from sample_index-1
        i = filter->sample_index-1;

        // if the next element is higher than our new sample, push it up one position
        while (i > 0 && filter->samples[i-1] > new_sample) {
            filter->samples[i] = filter->samples[i-1];
            i--;
        }

        // add our new sample to the buffer
        filter->samples[i] = new_sample;

    } else {    // drop lowest sample from the buffer to make room for our new sample

        // start from the bottom
        i = 0;

        // if the element is lower than our new sample, push it down one position
        while ( i < filter->sample_index-1 && filter->samples[i+1] < new_sample ) {
            filter->samples[i] = filter->samples[i+1];
            i++;
        }

        // add our new sample to the buffer
        filter->samples[i] = new_sample;
    }
}
/*------------------------------------test------------------------------------*/


